/*
 * main.cpp
 *
 *  Created on: Mar 8, 2011
 *      Author: Jason
 */

#include "axis.h"
#include "stepper.h"
#include "bed.h"
#include "stddef.h"
#include "serial.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "Gcode.h"

#include "NokiaLCD.h"

//#define DEBUG

#define XSTEP	PD,7
#define XDIR	PC,2
#define XEN		PC,3
#define XMIN	PC,4

#define YSTEP	PC,7
#define YDIR	PC,6
#define YEN		PA,7
#define YMIN	PA,6

#define ZSTEP	PA,4
#define ZDIR	PA,3
#define ZEN		PA,2
#define ZMIN	PA,1

#define ESTEP	PA,5  //Old X Max Pin
#define EDIR	PC,5  //Old Y Max Pin
#define EEN		PA,0  //Old Z Max Pin

#define LCD_CS	PB,0

#define RS485RXEN PD,5
#define RS485TXEN PD,4


#define X_STEPS_MM 10.4063737
#define Y_STEPS_MM 10.3393656
#define Z_STEPS_MM 678.45759551
#define E_STEPS_MM 74.2810372
#define E_EXTR_RATIO 0.01


	axis 	xAxis,
			yAxis,
			zAxis,
			eAxis;

	cartesian bed;

	gEngine gcode;
	
	long v,a;

int main(void)
{
	int i;
	
	unsigned char start[10];
	unsigned char ok[5];
	unsigned char str[128];

#ifdef DEBUG	
	unsigned char lcdbuff[64];
#endif

	gRecord *rcd;
	
	unsigned char len;
	
	NokiaLCD lcd;
	
	pin rs485rxEn;
	pin rs485txEn;
	
	initTimer0();
	
#ifdef DEBUG
	lcd.init();
	lcd.clearScreen(BLACK);
	lcd.drawString("RepRap Rage",RED, BLACK, 10,10);
	delay(5000);
#endif

	xAxis.drive.setupStep(XSTEP);
	xAxis.drive.setupDir(XDIR);
	xAxis.drive.setupEn(XEN);
	xAxis.setupMin(XMIN);
	xAxis.drive.invertDir(FALSE);
	xAxis.drive.invertEn(TRUE);
	xAxis.invertMin(TRUE);
	xAxis.setMaxSteps(X_STEPS_MM*200);
	xAxis.setVelMax(100000);
	xAxis.setAcc(100000);

	yAxis.drive.setupStep(YSTEP);
	yAxis.drive.setupDir(YDIR);
	yAxis.drive.setupEn(YEN);
	yAxis.setupMin(YMIN);
	yAxis.drive.invertDir(FALSE);
	yAxis.drive.invertEn(TRUE);
	yAxis.invertMin(TRUE);
	yAxis.setMaxSteps(Y_STEPS_MM*200);
	yAxis.setVelMax(100000);
	yAxis.setAcc(100000);

	zAxis.drive.setupStep(ZSTEP);
	zAxis.drive.setupDir(ZDIR);
	zAxis.drive.setupEn(ZEN);
	zAxis.setupMin(ZMIN);
	zAxis.drive.invertDir(TRUE);
	zAxis.drive.invertEn(TRUE);
	zAxis.invertMin(TRUE);
	zAxis.setMaxSteps(Z_STEPS_MM*50);
	zAxis.setVelMax(2000);
	xAxis.setAcc(1000);

//Extruder
	eAxis.drive.setupStep(ESTEP);
	eAxis.drive.setupDir(EDIR);
	eAxis.drive.setupEn(EEN);
	eAxis.drive.invertDir(TRUE);
	eAxis.drive.invertEn(TRUE);
	eAxis.setMaxSteps(9999999);  //Disable max step
	eAxis.setVelMax(7000);
	eAxis.setRef(RELATIVE);

	xAxis.drive.set(ENABLE);
	yAxis.drive.set(ENABLE);
	zAxis.drive.set(ENABLE);
	eAxis.drive.set(ENABLE);
	
	eAxis.move(100);
	
	eAxis.move(0);
			
	bed._x = &xAxis;
	bed._y = &yAxis;
	bed._z = &zAxis;
	bed._e = &eAxis;
	bed._extRatio = E_EXTR_RATIO * E_STEPS_MM;
	 	
	gcode._bed = &bed;
	gcode.setScale(X_STEPS_MM, Y_STEPS_MM, Z_STEPS_MM, E_STEPS_MM);
	serialInit();
		
	bed.zero(2000,2000,2000,100);

	sprintf((char *)start,"start\n");			
	rsWrite(start, strlen((char *)start));
	sprintf((char*)ok, "ok\n");
	rsWrite(ok, strlen((char *)ok));

	while(1)	
	{
		len = rsRead(str);
		if(len)
		{
			gcode.cue(str);
			if(gcode._buff.available()>1)
				rsWrite(ok, strlen((char*)ok));

#ifdef DEBUG				
			rcd = gcode._buff.peek();
			if(rcd)			
			{
				lcd.clearScreen(BLACK);
				lcd.drawString((char *) str, GREEN, BLACK, 1, 1);
				if(rcd->getb(G))
				{
					sprintf((char *)lcdbuff,"G: %d",abs(rcd->get(G)));
					lcd.drawString((char *)lcdbuff,RED,BLACK,1,11);
				}					
				if(rcd->getb(X))
				{
					sprintf((char *)lcdbuff,"X: %d",abs(rcd->get(X))*100);
					lcd.drawString((char *)lcdbuff,PINK,BLACK,1,21);
				}					
				if(rcd->getb(Y))
				{
					sprintf((char *)lcdbuff,"Y: %d",abs(rcd->get(Y)*100));
					lcd.drawString((char *)lcdbuff,ORANGE,BLACK,1,31);
				}					
				if(rcd->getb(Z))
				{
					sprintf((char *)lcdbuff,"Z: %d",abs(rcd->get(Z)*100));
					lcd.drawString((char *)lcdbuff,YELLOW,BLACK,1,41);
				}					
				if(rcd->getb(E))
				{
					sprintf((char *)lcdbuff,"E: %d",abs(rcd->get(E)*100));
					lcd.drawString((char *)lcdbuff,WHITE,BLACK,1,51);
				}		
				if(rcd->getb(F))
				{
					sprintf((char *)lcdbuff,"F: %d",abs(rcd->get(F)*100));
					lcd.drawString((char *)lcdbuff,WHITE,BLACK,1,61);
				}		
			}		
#endif
		}		
		gcode.process();
		len = strlen((char *)gcode._reply);
		if(len)
			rsWrite(gcode._reply, len);
	}
	
}
